#include "include/subdivision_of_polygon.h"
static inline int normalize(double *a)
{
    double norm=a[0]*a[0]+a[1]*a[1]+a[2]*a[2];
    if(norm==0||fabs(norm)<1e-9)
    {
       return 0; 
    }
    norm=SAFE_SQRT(norm);
    if(norm==0||fabs(norm)<1e-9)
    {
        return 0;
    } 
    a[0]/=norm;a[1]/=norm;a[2]/=norm;
    return 1;
}

static inline double *out_product(double *a,double*b)
{
    double* re=(double*)malloc(sizeof(double)*3);
    
    re[0]=a[1]*b[2]-a[2]*b[1];
    re[1]=-a[0]*b[2]+a[2]*b[0];
    re[2]=a[0]*b[1]-a[1]*b[0];
    return re; 
}
static inline double inner_product(double* a,double* b)
{ 
    return a[0]*b[0]+a[1]*b[1]+a[2]*b[2];
}
// is piont is in line ?
//
static inline int is_point_in_line(double *l,double*p)
{

    double temp_p[3]={p[0]-l[3],p[1]-l[4],p[2]-l[5]};
    double tempd=inner_product(l,temp_p);
    tempd=(temp_p[0]*temp_p[0]+temp_p[1]*temp_p[1]+temp_p[2]*temp_p[2])-tempd*tempd;
    if(tempd<1e-8)
    {
        return 1;
    } 
    return 0;
}

// is line intersecting with line on one point
// return 4 when only v2 lying in l 
// return 3 when only v1 lying in l 
// return 2 when line segment is falling in line(v1 v2 lying in l )
// return 1 when line segment is intersecting with one piont 
// return 0 when there is no intersection

static inline int is_line_intersecting_with_line_segment(double* l,double* v1,double *v2)
{
    int bool_1=is_point_in_line(l,v1),bool_2=is_point_in_line(l,v2);

    if(bool_1==1)
    {
        if(bool_2==1)
        {
            return 2;
        }
        else
        {
            return 3;
        }
    }
    else if(bool_2==1)
    {
        return 4;
    } 

    double d1[3]={v1[0]-l[3],v1[1]-l[4],v1[2]-l[5]};
    double d2[3]={v2[0]-l[3],v2[1]-l[4],v2[2]-l[5]};

    double * n1=out_product(l,d1);
    double* n2=out_product(l,d2);
    //printf("n2 %lf %lf %lf\n",n2[0],n2[1],n2[2] ); 
    //printf("n1 %lf %lf %lf\n",n1[0],n1[1],n1[2] ); 
    if(inner_product(n1,n2)<=1e-9)
    {
        free(n1);free(n2);
        return 1;
    } 
    free(n1);free(n2);
    return 0;
}
// @ l: the normalized line n1*t1+p1
// @ l1:the normalized line n2*t2+p2
// @ return the t1   
static inline double line_point_nearest_2_line(double* l,double* l1)
{
    double n1n2=inner_product(l,l1);
    double p1p2[3]={l[3]-l1[3],l[4]-l1[4],l[5]-l1[5]};

    return (n1n2* inner_product(p1p2,l1)-inner_product(p1p2,l))/(1-(n1n2)*(n1n2));
}
static inline double * get_line_equation_from_two_p(double*p1,double* p2 )
{
    double * re=(double*)malloc(sizeof(double)*6);
    re[0]=p2[0]-p1[0];re[1]=p2[1]-p1[1];re[2]=p2[2]-p1[2];
    if(!normalize(re))
    {
        free(re);
        return NULL;
    } 
    re[3]=p1[0];re[4]=p1[1];re[5]=p1[2];

    return re;
}




typedef struct Sub_Poly_Ver{
    double* p;
    int i;
}Sub_Poly_Ver;
static inline sub_poly_ver_init(Sub_Poly_Ver* spv)
{
    spv->p=NULL;
    spv->i=0;
}
static void subdivision_of_polygon2( Sub_Poly_Ver* spvs,int len,int **s)
{
    if(len<3)
    {
        return ;
    }
    if(len==3)
    {
        s[0][0]=spvs[0].i;
        s[0][1]=spvs[1].i;
        s[0][2]=spvs[2].i;
        return ;
    }
    int an=0;
    double** v=(double**)malloc(sizeof(double*)*len);
    for(int i=0;i<len;i++)
    {
        v[i]=spvs[i].p;
       // printf("%lf %lf %lf\n",v[i][0],v[i][1],v[i][2]);
    } 

    for(int i=0;i<len;i++)
    {
        if(is_subdivision_of_polygon_legal_angle(v,len,i))
        {
            an=i;
            break;
        } 
    }

   // printf("8****************************************************8an an:%d\n",an );
    free(v);
   // printf("here1\n");
    int ia=(an-1+len)%len,ib=(an+1+len)%len;
    
    s[0][0]=spvs[ia].i;
    s[0][1]=spvs[an].i;
    s[0][2]=spvs[ib].i;
    
    int temp_i=0;
    Sub_Poly_Ver* spvs1=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*(len-1));
    for(int i=0;i<len;i++)
    {
        if(i==an)
        {
            continue;
        }
        spvs1[temp_i]=spvs[i];
      
     
        temp_i++;
       
    }
    subdivision_of_polygon2(spvs1,len-1,&(s[1]));
    free(spvs1);

}
void subdivision_of_polygon(double ** v,int len,int ** s)
{
    Sub_Poly_Ver* spvs=(Sub_Poly_Ver*)malloc(sizeof(Sub_Poly_Ver)*len);
    for(int i=0;i<len;i++)
    {
        spvs[i].p=v[i];
        spvs[i].i=i;
    } 
    subdivision_of_polygon2(spvs,len,s); 
    free(spvs);

}

static inline int is_three_point_in_one_line(double* p1,double* p2,double* p3)
{
    double temp_p1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    double temp_p2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};
    //normalize 
    double* p= out_product(temp_p1,temp_p2);
    int re=0;
    if(fabs(p[0])<1e-9&&fabs(p[1])<1e-9&&fabs(p[2])<1e-9)
    {re=1;}
    SAFE_FREE(p);   
    return re; 
}
void optimization_subdivision_of_polygon(double** v,int len ,int **s)
{
    if(len==3)
    {
        s[0][0]=0;s[0][1]=1;s[0][2]=2;
        return ;
    }
    int *mark=(int*)malloc(sizeof(int)*len);
    for(int i=0;i<len;i++)
    {
         mark[i]=i;
    }
    int len1=0;
    for(int i=0;i<len;i++)
    {
        int j=(i+1)%len,k=(i+2)%len;
        if(is_three_point_in_one_line(v[i],v[j],v[k])==1)
        {
            mark[j]=-1;
            len1++;
        }
    } 
    double** v1=(double**)malloc(sizeof(double*)*(len-len1));
    int *index=(int*)malloc(sizeof(int)*(len-len1));
    int temp_i=0;
    for(int i=0;i<len;i++)
    {
        if(mark[i]!=-1)
        {
            v1[temp_i]=v[i];
            index[temp_i]=i;
            temp_i++;
        }
    }
    subdivision_of_polygon(v1,len-len1,s);
    int len2=len-len1-2;
    for(int i=0;i<len2;i++)
    {
        for(int j=0;j<3;j++)
        {
            s[i][j]=index[s[i][j]];
        }
    }

    //alter s

    for(int i=0;i<len;i++)
    {
        if(mark[i]==-1)
        {
            int k1=i;
            for(int j=1;j<len;j++)
            {
                k1=(i-j+len)%len;
                if(mark[k1]!=-1)
                {
                    break;
                }  
            }
            int k2=i;
            for(int j=1;j<len;j++)
            {
                k2=(i+j)%len;
                if(mark[k2]!=-1)
                {
                    break;
                }
            }
            int l1=-1,l2=-1;
            for(int j=0;j<len2;j++)
            {
                for(int k=0;k<3;k++)
                {
                    if(s[j][k]==k1)
                    {
                        l1=k;
                        break;
                    }
                }
                for(int k=0;k<3;k++)
                {
                    if(s[j][k]==k2)
                    {
                        l2=k;
                        break;
                    }
                }
                if(l1!=-1&&l2!=-1)
                {
                    s[len2][0]=s[j][0];s[len2][1]=s[j][1];s[len2][2]=s[j][2];
                    s[j][l1]=i;
                    s[len2][l2]=i;
                    break;
                }
            }
            mark[i]=i;
            len2++;
        }
    }
    free(index);
    free(v1);
    free(mark);
}